#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>

void laserScanMsgCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    int length = msg->ranges.size();
    ROS_INFO("laser scan msg length: %d", length);
}

void odomMsgCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    ROS_INFO("odom msg pose:[%f,%f]", msg->pose.pose.position.x, msg->pose.pose.position.y);
    ROS_INFO("odom msg twist:[%f,%f]", msg->twist.twist.linear.x, msg->twist.twist.linear.y);
    double siny = 2.0 * (msg->pose.pose.orientation.w * msg->pose.pose.orientation.z + msg->pose.pose.orientation.x * msg->pose.pose.orientation.y);
	double cosy = 1.0 - 2.0 * (msg->pose.pose.orientation.y * msg->pose.pose.orientation.y + msg->pose.pose.orientation.z * msg->pose.pose.orientation.z);  

	atan2(siny, cosy);
}

int main(int arg, char **args)
{
    ros::init(arg, args, "demo_drive");
    ros::NodeHandle node;

    ros::Subscriber laser = node.subscribe("scan", 100, laserScanMsgCallback);
    ros::Subscriber odom = node.subscribe("odom", 100, odomMsgCallback);

    ros::spin();
    
    return 0;
}


